#include <bits/stdc++.h>
#include "main.h"
#include "astar.h"
#include "init.h"
#include "robot.h"
using namespace std;


// A*算法，计算结果通过ans返回,type=0则不考虑机器人坐标，type=1则考虑机器人坐标
void Astar(int x_start, int y_start, int x_end, int y_end, vector<Point> &ans, int type)
{
	if (ans.size() != 0) {
		ans.clear();
	}
	++astar_times;
    priority_queue<Node> open; // 优先队列，小顶堆，优先输出F或G最小的节点
    bool is_vist[n][n] = { 0 };  // 记录节点是否被访问过
    int valueF[n][n] = { 0 };    // 记录每个节点对应的F值
    list<Node> parent;         // 记录父节点 
    int flag = 0;

    // 将起点加入open列表
    Node node_begin(x_start, y_start);
    Node node_end(x_end, y_end);
    node_begin.H = Manhattan(node_begin.p, node_end.p);
    node_begin.F = node_begin.H;
    open.push(node_begin);

    while (!open.empty()) {
        Node node_cur = open.top(); // 取小顶堆的顶部元素，即代价F最小的点
        open.pop(); // 将该点从堆中移除
        is_vist[node_cur.p.x][node_cur.p.y] = true; // 访问该点
        parent.push_back(node_cur);

        // 到达终点
        if (node_cur.p.x == x_end && node_cur.p.y == y_end) {
            flag = 1;
            break;
        }

        // 遍历node_cur周围的4个位置
        for (int i = 0; i < 4; i++) {
            int x_next = node_cur.p.x + next_position[i][0];
            int y_next = node_cur.p.y + next_position[i][1];
            // 该节点坐标合法，且没有被访问
            if (type == 0) 
            {
		        if (x_next >= 0 && x_next < n && y_next >= 0 && y_next < n && ch_map[x_next][y_next] != '#' && ch_map[x_next][y_next] != '*' && !is_vist[x_next][y_next]) 
		        {
		            Node node_next(x_next, y_next);
		            node_next.G = node_cur.G + 1;
		            node_next.H = Manhattan(node_next.p, node_end.p);
		            node_next.F = node_next.G + node_next.H;

		            // node_next.F < valueF[node_next.x][node_next.y] 说明找到了更优的路径，进行更新
		            // valueF[node_next.x][node_next.y] == 0 说明该节点还未加入open表中，则加入
		            if (node_next.F < valueF[x_next][y_next] || valueF[x_next][y_next] == 0) 
		            {
		                // 保存该节点的父节点
		                node_next.father = &(*parent.rbegin());
		                valueF[x_next][y_next] = node_next.F;
		                open.push(node_next);
		            }
		        }
            } 
            else
            {
            	if (x_next >= 0 && x_next < n && y_next >= 0 && y_next < n && ch_map[x_next][y_next] != '#' && ch_map[x_next][y_next] != '*' && ch_map[x_next][y_next] != 'A' && !is_vist[x_next][y_next]) 
		        {
		            Node node_next(x_next, y_next);
		            node_next.G = node_cur.G + 1;
		            node_next.H = Manhattan(node_next.p, node_end.p);
		            node_next.F = node_next.G + node_next.H;

		            // node_next.F < valueF[node_next.x][node_next.y] 说明找到了更优的路径，进行更新
		            // valueF[node_next.x][node_next.y] == 0 说明该节点还未加入open表中，则加入
		            if (node_next.F < valueF[x_next][y_next] || valueF[x_next][y_next] == 0) 
		            {
		                // 保存该节点的父节点
		                node_next.father = &(*parent.rbegin());
		                valueF[x_next][y_next] = node_next.F;
		                open.push(node_next);
		            }
		        }
            }
        }
    }

    // 没有找到通往目标点的路径，返回空数组
    if (flag == 0)
        return;

    Node *temp = &(*(parent.rbegin()));
    while (temp->father != NULL) {
        ans.push_back(temp->p);
        temp = temp->father;
    }
    ans.push_back(temp->p);
}

// A*算法
void astar_conflict(int x_start, int y_start, int x_end, int y_end, vector<Point> &ans, int type)
{
	if (ans.size() != 0) {
		ans.clear();
	}
    priority_queue<Node> open; // 优先队列，小顶堆，优先输出F或G最小的节点
    bool is_vist[n][n] = { 0 };  // 记录节点是否被访问过
    int valueF[n][n] = { 0 };    // 记录每个节点对应的F值
    list<Node> parent;         // 记录父节点 
    int flag = 0;

    // 将起点加入open列表
    Node node_begin(x_start, y_start);
    Node node_end(x_end, y_end);
    node_begin.H = Manhattan(node_begin.p, node_end.p);
    node_begin.F = node_begin.H;
    open.push(node_begin);

    while (!open.empty()) {
        Node node_cur = open.top(); // 取小顶堆的顶部元素，即代价F最小的点
        open.pop(); // 将该点从堆中移除
        is_vist[node_cur.p.x][node_cur.p.y] = true; // 访问该点
        parent.push_back(node_cur);

        // 到达终点
        if (node_cur.p.x == x_end && node_cur.p.y == y_end) {
            flag = 1;
            break;
        }

        // 遍历node_cur周围的4个位置
        for (int i = 0; i < 4; i++) {
            int x_next = node_cur.p.x + next_position[i][0];
            int y_next = node_cur.p.y + next_position[i][1];
            // 该节点坐标合法，且没有被访问
            if (type == 0) 
            {
		        if (x_next >= 0 && x_next < n && y_next >= 0 && y_next < n && ch_map[x_next][y_next] != '#' && ch_map[x_next][y_next] != '*' && !is_vist[x_next][y_next]) 
		        {
		            Node node_next(x_next, y_next);
		            node_next.G = node_cur.G + 1;
		            node_next.H = Manhattan(node_next.p, node_end.p);
		            node_next.F = node_next.G + node_next.H;

		            // node_next.F < valueF[node_next.x][node_next.y] 说明找到了更优的路径，进行更新
		            // valueF[node_next.x][node_next.y] == 0 说明该节点还未加入open表中，则加入
		            if (node_next.F < valueF[x_next][y_next] || valueF[x_next][y_next] == 0) 
		            {
		                // 保存该节点的父节点
		                node_next.father = &(*parent.rbegin());
		                valueF[x_next][y_next] = node_next.F;
		                open.push(node_next);
		            }
		        }
            } 
            else
            {
            	if (x_next >= 0 && x_next < n && y_next >= 0 && y_next < n && ch_map[x_next][y_next] != '#' && ch_map[x_next][y_next] != '*' && ch_map[x_next][y_next] != 'A' && !is_vist[x_next][y_next]) 
		        {
		            Node node_next(x_next, y_next);
		            node_next.G = node_cur.G + 1;
		            node_next.H = Manhattan(node_next.p, node_end.p);
		            node_next.F = node_next.G + node_next.H;

		            // node_next.F < valueF[node_next.x][node_next.y] 说明找到了更优的路径，进行更新
		            // valueF[node_next.x][node_next.y] == 0 说明该节点还未加入open表中，则加入
		            if (node_next.F < valueF[x_next][y_next] || valueF[x_next][y_next] == 0) 
		            {
		                // 保存该节点的父节点
		                node_next.father = &(*parent.rbegin());
		                valueF[x_next][y_next] = node_next.F;
		                open.push(node_next);
		            }
		        }
            }
        }
    }

    // 没有找到通往目标点的路径，返回空数组
    if (flag == 0)
        return;

    Node *temp = &(*(parent.rbegin()));
    while (temp->father != NULL) {
        ans.push_back(temp->p);
        temp = temp->father;
    }
    ans.push_back(temp->p);
}
